public class Robot {
	
	public static int x;
	public static int y;
	public static int direction;
	
	public static void initialize(){
		x = 0;
		y = 0;
		direction = Constants.RIGHT;
		updateGrid();
		Data.currentFloor.grids[x][y].seen=1;
	}
	
	public static void motionUpdate() {	
		if(direction == Constants.RIGHT)
			x++;
		else if(direction == Constants.DOWN)
			y++;
		else if(direction == Constants.LEFT)
			x--;
		else if(direction == Constants.UP)
			y--;

		updateMoveList();
		
		///////READ SENSOR VALUES//////
		
		if(Data.currentFloor.grids[x][y].seen == 0) {
			//Read color here
			updateGrid();
		}
		
		Data.currentFloor.grids[x][y].seen++;
		//TODO: read color value from light sensor
		/*int colorType = ColorDetector.getType(color);
			  useColors(colorType);
		 */

	}
	
	public void detectObstacles(int distance){

		int index = 0;
		int limit = 1;
		int detectDist = Constants.GRID_SIZE / 2;

		while(index < limit && distance > detectDist){
			updateObstacle(index, false);
			index++;
			distance -= Constants.GRID_SIZE;
		}
		if(index != limit)
			updateObstacle(index, true);
		//		else
		//			detectObstacle(index, false);
	}
	
	public static void updateGrid() {
		int colorType = Data.currentFloor.grids[x][y].color;
		
		if(colorType==Constants.FIRST_ROOM)
			Data.saysFirst++;
		else if(colorType==Constants.SECOND_ROOM)
			Data.saysSecond++;
		else if(colorType==Constants.KING){
			Data.kingSeen=true;
			Data.xKing = x;
			Data.yKing = y;
			Data.kingSeenMove = Data.moveList.size();
		}
	}
	
	public void updateObstacle(int k, boolean exists){
		if(direction == Constants.RIGHT){
			Data.currentFloor.setObstacle(x + k, y, exists, true);
		}
		else if(direction == Constants.DOWN){
			Data.currentFloor.setObstacle(x, y + k, exists, false);
		}
		else if(direction == Constants.LEFT){
			Data.currentFloor.setObstacle(x - 1 - k, y, exists, true);
		}
		else if(direction == Constants.UP){
			Data.currentFloor.setObstacle(x, y - 1 - k, exists, false);
		}
		
	}
	
	public static void updateMoveList(){
		Data.moveList.add(new Move(direction));
	}
	
	public static void rotateTo(int heading){
		direction = heading;
	}
}